import socket
import time
import pandas as pd
 
#服务器
host_ip = '192.168.1.244'
port1 = 6633
port2 = 6666
addr1 = (host_ip, port1)
addr2 = (host_ip,port2)
server_lidar = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)  # 声明socket类型，同时生成链接对象
server_lidar.bind(addr1)
server_gps = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)  # 声明socket类型，同时生成链接对象
server_gps.bind(addr2)


#字节转16进制
def decode_tohex(a):
    return ord(a[0])

#16进制转字符串
def decode_tostring(b):

    return chr(ord(b[0]))    

#雷达解码(返回一个接近360度的数据；格式为pd.DataFrame)
def lidar(flag_finish=17):
    rx_data=None
    data_return=pd.DataFrame(columns=["角度","距离"])
    flag_ready=0
    while(flag_finish):
        #包头
        if(rx_data!=0xAA):
            rx_data=decode_tohex(server_lidar.recvfrom(100))
        
        if(rx_data==0xAA):
            rx_data=decode_tohex(server_lidar.recvfrom(100))
            if(rx_data==0x55):
                flag_ready=1

        if(flag_ready):
            #包类型
            rx_data=decode_tohex(server_lidar.recvfrom(100))
            #点云数据包
            if(rx_data==0x00):
                #采样数量
                data_length=decode_tohex(server_lidar.recvfrom(100))
                if(data_length==1):
                    data_length+=1
                #角度信息
                rx_data=[None]*4
                for i in range(0,4):
                    rx_data[i]=decode_tohex(server_lidar.recvfrom(100))
                data_angle_begin=(rx_data[0]+rx_data[1]*256)/128
                data_angle_end=(rx_data[2]+rx_data[3]*256)/128
                if(data_angle_end>data_angle_begin):
                    data_angle_dif=data_angle_end-data_angle_begin
                else:
                    data_angle_dif=data_angle_end+360-data_angle_begin
                data_angle=[None]*data_length
                for i in range(0,data_length):
                    data_angle[i]=(data_angle_dif/(data_length-1))*i+data_angle_begin
                    if(data_angle[i]>360):
                        data_angle[i]-=360
                #校验码
                rx_data=server_lidar.recvfrom(100)
                rx_data=server_lidar.recvfrom(100)
                #采样数据
                rx_data=[None]*(data_length*2)
                for i in range(0,data_length*2):
                    rx_data[i]=decode_tohex(server_lidar.recvfrom(100))
                data_distance=[None]*data_length
                for i in range(0,data_length):
                    data_distance[i]=rx_data[2*i]+rx_data[2*i+1]*256
                #数据输出
                data=pd.DataFrame({"角度":data_angle,"距离":data_distance})
                data_return=data_return.append(data)
                flag_finish-=1
            else:
                flag_ready=0
    data_return=data_return.reset_index(drop=True)
    return data_return

#GPS 解码(返回定位信息；格式为列表)
def gps(flag_finish=1):
    key="$GNRMC"
    rx_data=None
    flag_ready=0
    list_data=[]
    while(flag_finish):
        if(rx_data!="$"):
            rx_data=decode_tostring(server_gps.recvfrom(100))
        if(rx_data=="$"):
            list_data.append(rx_data)
            flag_ready=1
        while(flag_ready):
            rx_data=decode_tostring(server_gps.recvfrom(100))
            list_data.append(rx_data)
            if(rx_data=="*"):
                flag_ready=0
                list_data="".join(list_data)
                list_data=list_data.split(",")
                if(list_data[0]==key):
                    flag_finish-=1
                else:
                    list_data=[]
    return list_data

           
#主函数
while True:
    print(lidar())
    print(gps())